

#include "ros/ros.h"
#include "ghor_hardware/SetMotorCmd.h"
#include <cstdlib>


static const int NSEC_PER_SECOND = 1e+9;


static inline double now()
{
   struct timespec n;
   clock_gettime(CLOCK_MONOTONIC, &n);
   return double(n.tv_nsec) / NSEC_PER_SECOND + n.tv_sec;
}



void callCmd(ros::ServiceClient client,ghor_hardware::SetMotorCmd service){

  if (client.call(service))
  {
    //ROS_INFO("Sum:");
  }
  else
  {
    //ROS_ERROR("Failed to call service add_two_ints");
    
  }

  

}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "testmotor");

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<ghor_hardware::SetMotorCmd>("motorCmd");
  ghor_hardware::SetMotorCmd srv;
  srv.request.motor1 =10;
  srv.request.motor2 =20;
  srv.request.motor3 =30;
  srv.request.motor4 =40;
  
  
  double start = now();
 
  for( int i=0;i<1;i++){
     callCmd(client,srv);
  }


  double end = now();

 ROS_ERROR("Time %f",(end-start));    

  return 0;
}







